#include <stdlib.h>
#include <stdio.h>
#include <fstream>
#include <iostream>
#include <iomanip>
#include "TROOT.h"
#include "TStyle.h"
#include "TMath.h"
#include "TCanvas.h"
#include "TRandom.h"
#include "TH1.h"
#include "TH2.h"
#include "TF1.h"
#include "TF2.h"
#include "TArc.h"
#include "TPolyLine3D.h"
#include "TPolyMarker3D.h"
#include "TView3D.h"
#include "TAxis3D.h"
#include "TTUBE.h"
#include "TNode.h"
#include "THelix.h"
#include "TGeometry.h"
#include "TLegend.h"
#include "TLine.h"
#include "TFile.h"

using namespace std;

typedef struct{
  double x;
  double y;
}Point2D;
typedef struct{
  double x;
  double y;
  double z;
}Point3D;

void SubVector2D( Point2D vec1, Point2D vec2, Point2D *vec );
void SubVector3D( Point3D vec1, Point3D vec2, Point3D *vec );
double SizeOfVector( Point3D vec );
void ScaleVector(Point3D *vec, double Scale);
void CrossProduct3D( Point3D vec1, Point3D vec2, Point3D *vec );
void NormalVector3D( Point3D pos1, Point3D pos2, Point3D pos3, Point3D *normal );
double DotProduct3D( Point3D vec1, Point3D vec2 );
int LinePolyCrossPoint( Point3D pos1, Point3D vec1, Point3D pos2, Point3D vec2, Point3D *pos );

void MuonRate(){

  //----------------------------------
  gRandom->SetSeed(time(NULL));

  float pi=TMath::Pi();
  const int nScinti= 2;
  const int nMulti = 2;
  const int ntime  = 100;

  float AlignScinti[nScinti][3]={
    { -8, -17, 52.7-20},{  -8, 17, 74.7+15}
    //{ -8, -40, 20},{  -8, 40, 100}
  };
  float SizeOfScinti[2]={ 30, 15};
  //  float SizeOfScinti[2]={ 16+6.5, 14+1.5};

  float NormalVector[nScinti][3]={
    {16,25,0},{16,25,0}
    //{0,1,1},{0,1,1}
  }; 
  float SideVector[nScinti][3]={
    {25,-16,0},{25,-16,0}
  }; 

  TFile *fwrite=new TFile("./Rate.root","recreate");

  Point3D NormalVec[nScinti],LongSideVec[nScinti],ShortSideVec[nScinti];
  for(int ii=0;ii<nScinti;ii++){
    /* */
    NormalVec[ii].x = NormalVector[ii][0];
    NormalVec[ii].y = NormalVector[ii][1];
    NormalVec[ii].z = NormalVector[ii][2];
    ScaleVector( &NormalVec[ii], 1 );
    /* */
    LongSideVec[ii].x = SideVector[ii][0];
    LongSideVec[ii].y = SideVector[ii][1];
    LongSideVec[ii].z = SideVector[ii][2];
    ScaleVector( &LongSideVec[ii], 1 );
    /* */
    CrossProduct3D(NormalVec[ii],LongSideVec[ii],&ShortSideVec[ii]);
    ScaleVector( &ShortSideVec[ii], 1 );
    /* Check Orthogonal*/
    if( DotProduct3D(NormalVec[ii], LongSideVec[ii])!=0 ){cout<<"Not Orthogonal with Long Side"<<endl;return;}
    if( DotProduct3D(NormalVec[ii],ShortSideVec[ii])!=0 ){cout<<"Not Orthogonal with Short Side"<<endl;return;}
  }
  // cout<< "    Normal Vector : ("<<NormalVec[0].x<<","<<NormalVec[0].y<<","<<NormalVec[0].z<<")"<<endl;
  // cout<< " Long Side Vector : ("<<LongSideVec[0].x<<","<<LongSideVec[0].y<<","<<LongSideVec[0].z<<")"<<endl;
  // cout<< "Short Side Vector : ("<<ShortSideVec[0].x<<","<<ShortSideVec[0].y<<","<<ShortSideVec[0].z<<")"<<endl;

  Point3D Pos[nScinti][4];
  for(int ii=0;ii<nScinti;ii++){
    for(int jj=0;jj<4;jj++){
      if(jj==0){
	Pos[ii][jj].x = AlignScinti[ii][0];
	Pos[ii][jj].y = AlignScinti[ii][1];
	Pos[ii][jj].z = AlignScinti[ii][2];
      }
      else if(jj==1){
	Point3D VecSide = LongSideVec[ii];
	ScaleVector( &VecSide, SizeOfScinti[0] );
	Pos[ii][jj].x = Pos[ii][jj-1].x + VecSide.x;
	Pos[ii][jj].y = Pos[ii][jj-1].y + VecSide.y;
	Pos[ii][jj].z = Pos[ii][jj-1].z + VecSide.z;
      }
      else if(jj==2){
	Point3D VecSide = ShortSideVec[ii];
	ScaleVector( &VecSide, SizeOfScinti[1] );
	Pos[ii][jj].x = Pos[ii][jj-1].x + VecSide.x;
	Pos[ii][jj].y = Pos[ii][jj-1].y + VecSide.y;
	Pos[ii][jj].z = Pos[ii][jj-1].z + VecSide.z;
      }
      else if(jj==3){
	Point3D VecSide = LongSideVec[ii];
	ScaleVector( &VecSide, SizeOfScinti[0] );
	Pos[ii][jj].x = Pos[ii][jj-1].x - VecSide.x;
	Pos[ii][jj].y = Pos[ii][jj-1].y - VecSide.y;
	Pos[ii][jj].z = Pos[ii][jj-1].z - VecSide.z;
      }
      //      if(ii==0)  cout<< "("<<Pos[0][jj].x<<","<<Pos[0][jj].y<<","<<Pos[0][jj].z<<")"<<endl;
    }
  }

  //---------------------------------------
  TCanvas *c1=new TCanvas("c1","c1",300,10,800,800);

  Double_t rmin[3]={-50,-50,  0};
  Double_t rmax[3]={ 50, 50, 150};
  Point3D tmpPos,tmpVec;
  tmpPos.x = 0;
  tmpPos.y = 0;
  tmpPos.z = rmax[2];
  tmpVec.x = 0;
  tmpVec.y = 0;
  tmpVec.z = 1;

  TView3D *view = new TView3D(1, rmin, rmax);
  view->SetLineColor(1);
  view->ShowAxis();
  view->SetOutlineToCube();

  TAxis3D *axis = TAxis3D::GetPadAxis(); // Get pointer to axis
  if(axis) {
    axis->SetLabelSize(0.02);    
    axis->SetLabelOffset(-0.02, "z"); 
    axis->SetLabelColor(1);  
    axis->SetAxisColor(1); 
    axis->SetXTitle("X");
    axis->SetYTitle("Y");
    axis->SetZTitle("Z");
  }

  TPolyLine3D *lgrid;
  int dx,dy;
  int ngrid=10;
  for(int igrid=0;igrid<ngrid;igrid++){
    lgrid=new TPolyLine3D(2);
    dx = (igrid+1)*(rmax[0]-rmin[0])/ngrid;
    int xx = rmin[0]+dx;
    lgrid->SetPoint(0,xx,rmin[1],0); 
    lgrid->SetPoint(1,xx,rmax[1],0); 
    lgrid->SetLineWidth(0.5);
    lgrid->Draw();

    lgrid=new TPolyLine3D(2);
    dy = (igrid+1)*(rmax[1]-rmin[1])/ngrid;
    int yy = rmin[1]+dy;
    lgrid->SetPoint(0,rmin[0],yy,0);
    lgrid->SetPoint(1,rmax[0],yy,0);
    lgrid->SetLineWidth(0.5);
    lgrid->Draw();
  }

  float zpos[2]={52.7,74.7};
  Point3D pAnode,pCathode,vecPad;
  pAnode.x=0;
  pAnode.y=0;
  pAnode.z=zpos[1];
  pCathode.x=0;
  pCathode.y=0;
  pCathode.z=zpos[0];
  vecPad.x=0;
  vecPad.y=0;
  vecPad.z=1;

  // TTUBE *JCtube=new TTUBE("JCtube","JCtube",0,14,15,52.5,1);
  // TGeometry *geo=new TGeometry("sample","sample");
  // geo->cd();
  // TNode *JChamber=new TNode("JChamber","JChamber",JCtube);
  // JChamber->SetPosition(0.,0.,105.);
  // geo->Draw();
  // JChamber->Add(JCtube,)
  // JCtube->Draw();

  //---------------------------------------
  int RangeX = 100; //(cm)
  int RangeY = 100; //(cm)
  int MidX = RangeX/2;
  int MidY = RangeY/2;
  TF1 *fcos2=new TF1("fcos2","pow(cos(x),2)",-1/2.*pi,1/2.*pi);

  TH1F *hCosmicPath=new TH1F("hCosmicPath","hCosmicPath",100,25,40);
  TH1F *hZenithAngle=new TH1F("hZenithAngle","hZenithAngle",100,-1*pi,pi);
  TH1F *hPhiAngle=new TH1F("hPhiAngle","hPhiAngle",100,0,pi);
  TH1F *hPosX=new TH1F("hPosX","hPosX",1000,-1*nMulti*MidX,nMulti*MidX);
  TH1F *hPosY=new TH1F("hPosY","hPosY",1000,-1*nMulti*MidY,nMulti*MidY);
  TH1F *hslopeX=new TH1F("hslopeX","hslopeX",100,0,10);
  TH1F *hslopeY=new TH1F("hslopeY","hslopeY",100,0,10);

  TH2F *hPos2D=new TH2F("hPos2D","hPos2D",100,-1*nMulti*MidX,nMulti*MidX,100,-1*nMulti*MidY,nMulti*MidY);

  TH2F *hScintiHitDistribution[nScinti];
  for(int ii=0;ii<nScinti;ii++){
    char tmpname[64];
    sprintf(tmpname,"hScintiHitDistribution%i",ii+1);
    hScintiHitDistribution[ii]=new TH2F(tmpname,tmpname,
					SizeOfScinti[0]*1.5,0,SizeOfScinti[0]*1.5,
					SizeOfScinti[1]*1.5,0,SizeOfScinti[1]*1.5);
  }
  TH2F *hAnodeHitDistribution=new TH2F("AnodeHit","AnodeHit",30,-15,15,30,-15,15);
  TH2F *hCathodeHitDistribution=new TH2F("CathodeHit","CathodeHit",30,-15,15,30,-15,15);

  float Z = 10;
  int AllCosmicCounter=0;
  int HitCosmicCounter=0;
  
  //for(int ix=0;ix<1;ix++){
  //   for(int iy=0;iy<1;iy++){
  for(int ix=-1*nMulti*MidX;ix<nMulti*MidX;ix++){
    for(int iy=-1*nMulti*MidY;iy<nMulti*MidY;iy++){

      for(int itime=0;itime<(int)ntime;itime++){
	float cellx= gRandom->Rndm()-0.5;
	float celly= gRandom->Rndm()-0.5;
	float PosX = ix+0.5 +cellx;
	float PosY = iy+0.5 +celly;
	float PosZ = 0;

	float phi  = 2*pi*gRandom->Rndm();
	float theta = fcos2->GetRandom();
	float coner = Z*tan(theta);

	float xx = coner*cos(phi) - PosX; 
	float yy = coner*sin(phi) - PosY; 
	float zz = Z;

	float slopeX = 1/(tan(theta)*cos(phi)); 
	float interceptX = -PosX/slopeX;
	float slopeY = 1/(tan(theta)*sin(phi));
 	float interceptY = -PosY/slopeY;

	AllCosmicCounter++;
	hPosX->Fill(PosX);
	hPosY->Fill(PosY);
	if( (int)(AllCosmicCounter/(RangeX*RangeY*ntime))*100 == 25 ) cout<<"------ 25% --------"<<endl;;
	if( (int)(AllCosmicCounter/(RangeX*RangeY*ntime))*100 == 50 ) cout<<"------ 50% --------"<<endl;;
	if( (int)(AllCosmicCounter/(RangeX*RangeY*ntime))*100 == 75 ) cout<<"------ 75% --------"<<endl;;

	Point3D PosPath,CosmicVec;
	PosPath.x = PosX;
	PosPath.y = PosY;
	PosPath.z = PosZ;
	CosmicVec.x = 1/slopeX;
	CosmicVec.y = 1/slopeY;
	CosmicVec.z = 1.;

	Point3D Intersection[nScinti];
	Point3D IntersectionXY[nScinti];
	int IsPathScinti[nScinti] = {0};
	Point3D VecAB[nScinti],VecAD[nScinti],VecAX[nScinti];
	double ss[nScinti],tt[nScinti];

	for(int ii=0;ii<nScinti;ii++){
	  int IsCross = LinePolyCrossPoint(PosPath,CosmicVec,Pos[ii][0],NormalVec[ii],&Intersection[ii]);

	  SubVector3D(Pos[ii][1],Pos[ii][0],&VecAB[ii]);
	  SubVector3D(Pos[ii][3],Pos[ii][0],&VecAD[ii]);
	  SubVector3D(Intersection[ii],Pos[ii][0],&VecAX[ii]);
	  float cosABtoAX = DotProduct3D(VecAB[ii],VecAX[ii])/(SizeOfVector(VecAB[ii])*SizeOfVector(VecAX[ii]));
	  float cosADtoAX = DotProduct3D(VecAD[ii],VecAX[ii])/(SizeOfVector(VecAD[ii])*SizeOfVector(VecAX[ii]));
	  float sizeAB = SizeOfVector(VecAB[ii]);
	  float sizeAD = SizeOfVector(VecAD[ii]);
	  float sizeAX = SizeOfVector(VecAX[ii]);
	  VecAB[ii].x=sizeAB;
	  VecAB[ii].y=0;
	  VecAB[ii].z=0;
	  VecAD[ii].x=0;
	  VecAD[ii].y=sizeAD;
	  VecAD[ii].z=0;
	  VecAX[ii].x = sizeAX*cosABtoAX;
	  VecAX[ii].y = sizeAX*cosADtoAX;
	  VecAX[ii].z = 0;
	  ss[ii] = (VecAD[ii].x*VecAX[ii].y-VecAD[ii].y*VecAX[ii].x)/(VecAB[ii].y*VecAD[ii].x-VecAB[ii].x*VecAD[ii].y);
	  tt[ii] = (VecAB[ii].y*VecAX[ii].x-VecAB[ii].x*VecAX[ii].y)/(VecAB[ii].y*VecAD[ii].x-VecAB[ii].x*VecAD[ii].y);
	  //cout<<"AB = "<<VecAB[ii].x<<endl;
	  /*
	  SubVector3D(Pos[ii][1],Pos[ii][0],&VecAB[ii]);
	  SubVector3D(Pos[ii][3],Pos[ii][0],&VecAD[ii]);
	  SubVector3D(Intersection[ii],Pos[ii][0],&VecAX[ii]);
	  if( (NormalVec[ii].y==0)&&(NormalVec[ii].z==0) ){
	    ss[ii] = (VecAD[ii].y*VecAX[ii].z-VecAD[ii].z*VecAX[ii].y)/(VecAB[ii].z*VecAD[ii].y-VecAB[ii].y*VecAD[ii].z);
	    tt[ii] = (VecAB[ii].z*VecAX[ii].y-VecAB[ii].y*VecAX[ii].z)/(VecAB[ii].z*VecAD[ii].y-VecAB[ii].y*VecAD[ii].z);
	  }else if( (NormalVec[ii].z==0)&&(NormalVec[ii].x==0) ){
	    ss[ii] = (VecAD[ii].z*VecAX[ii].x-VecAD[ii].x*VecAX[ii].z)/(VecAB[ii].x*VecAD[ii].z-VecAB[ii].z*VecAD[ii].x);
	    tt[ii] = (VecAB[ii].x*VecAX[ii].z-VecAB[ii].z*VecAX[ii].x)/(VecAB[ii].x*VecAD[ii].z-VecAB[ii].z*VecAD[ii].x);
	  }else{ //if( (NormalVec[ii].x==0)&&(NormalVec[ii].y==0) ){
	    ss[ii] = (VecAD[ii].x*VecAX[ii].y-VecAD[ii].y*VecAX[ii].x)/(VecAB[ii].y*VecAD[ii].x-VecAB[ii].x*VecAD[ii].y);
	    tt[ii] = (VecAB[ii].y*VecAX[ii].x-VecAB[ii].x*VecAX[ii].y)/(VecAB[ii].y*VecAD[ii].x-VecAB[ii].x*VecAD[ii].y);
	  }
	  */

	  if( (ss[ii]>0&&ss[ii]<1)&&(tt[ii]>0&&tt[ii]<1) ) IsPathScinti[ii] = 1;
	}

	/*Draw Cosmic Path*/	
	if(IsPathScinti[0]&&IsPathScinti[1]){

	  hZenithAngle->Fill(theta);
	  hPhiAngle->Fill(phi);
	  hslopeX->Fill(slopeX);
	  hslopeY->Fill(slopeY);
	  hPos2D->Fill(PosX,PosY);

	  Point3D CosmicPathVector;
	  SubVector3D(Intersection[1],Intersection[0],&CosmicPathVector);
	  hCosmicPath->Fill( SizeOfVector(CosmicPathVector) );

	  //Draw Cosmic Path
	  Point3D Edge;
	  int tmp = LinePolyCrossPoint(PosPath,CosmicVec,tmpPos,tmpVec,&Edge);
	  TPolyLine3D *CosmicPath=new TPolyLine3D(2);
	  CosmicPath->SetPoint(0,PosX,PosY,PosZ);
	  CosmicPath->SetPoint(1,Edge.x,Edge.y,rmax[2]);
	  CosmicPath->SetLineColor(kRed);
	  CosmicPath->Draw();

	  //
	  Point3D IntersectionA,IntersectionC;
	  int IsCrossAnode = LinePolyCrossPoint(PosPath,CosmicVec,pAnode,vecPad,&IntersectionA);
	  int IsCrossCathode = LinePolyCrossPoint(PosPath,CosmicVec,pCathode,vecPad,&IntersectionC);
	  hAnodeHitDistribution->Fill(IntersectionA.x,IntersectionA.y);
	  hCathodeHitDistribution->Fill(IntersectionC.x,IntersectionC.y);

	  for(int ii=0;ii<nScinti;ii++){
	    hScintiHitDistribution[ii]->Fill(VecAX[ii].x,VecAX[ii].y);
	  }
	  
	  HitCosmicCounter++;
	}
	
      }
      
    }
  }

  //---------------------------------------  
  TPolyLine3D *lAnode;
  float RR=13.3;
  for(int ii=0;ii<2;ii++){
    lAnode=new TPolyLine3D(2);
    float ang=50*2*(0.75*pi)/100.; 
    float xx=RR*cos(ang);
    float yy=RR*sin(ang);
    lAnode->SetPoint(0,xx,yy,zpos[ii]);
    ang=(99+51)*2*(0.75*pi)/100.;
    xx=RR*cos(ang);
    yy=RR*sin(ang);
    lAnode->SetPoint(1,xx,yy,zpos[ii]);
    //lAnode->Draw();

    for(int il=0;il<100;il++){
      lAnode=new TPolyLine3D(2);
      ang=(il+50)*2*(0.75*pi)/100.;
      xx=RR*cos(ang);
      yy=RR*sin(ang);
      lAnode->SetPoint(0,xx,yy,zpos[ii]);
      ang=(il+51)*2*(0.75*pi)/100.;
      xx=RR*cos(ang);
      yy=RR*sin(ang);
      lAnode->SetPoint(1,xx,yy,zpos[ii]);
      //lAnode->Draw();
    }
  }
  /*
  TPolyLine3D *lTopFlange;
  for(int il=0;il<100;il++){
    lTopFlange=new TPolyLine3D(2);
    float ang=il*2*pi/100.;
    float xx=16.5*cos(ang);
    float yy=16.5*sin(ang);
    lTopFlange->SetPoint(0,xx,yy,109.5);
    ang=(il+1)*2*pi/100.;
    xx=16.5*cos(ang);
    yy=16.5*sin(ang);
    lTopFlange->SetPoint(1,xx,yy,109.5);
    lTopFlange->Draw();
  }
  */
  TPolyLine3D *DrawScinti3D[nScinti];
  for(int ii=0;ii<nScinti;ii++){
    DrawScinti3D[ii]=new TPolyLine3D(5); 
    DrawScinti3D[ii]->SetPoint(0,Pos[ii][0].x,Pos[ii][0].y,Pos[ii][0].z);
    DrawScinti3D[ii]->SetPoint(1,Pos[ii][1].x,Pos[ii][1].y,Pos[ii][1].z);
    DrawScinti3D[ii]->SetPoint(2,Pos[ii][2].x,Pos[ii][2].y,Pos[ii][2].z);
    DrawScinti3D[ii]->SetPoint(3,Pos[ii][3].x,Pos[ii][3].y,Pos[ii][3].z);
    DrawScinti3D[ii]->SetPoint(4,Pos[ii][0].x,Pos[ii][0].y,Pos[ii][0].z);
    DrawScinti3D[ii]->SetLineColor(kBlue);
    DrawScinti3D[ii]->Draw();
  }
  //-------------------

  TCanvas *c2=new TCanvas("c2","c2",1200,600);
  gStyle->SetOptStat(000000);
  c2->SetGrid(1);
  c2->Divide(1,2);
  c2->cd(1);
  hZenithAngle->SetXTitle("#theta [rad]");
  hZenithAngle->SetYTitle("Arbitrary Unit");
  hZenithAngle->DrawNormalized();
  c2->cd(2);
  hPhiAngle->SetXTitle("#phi [rad]");
  hPhiAngle->SetYTitle("Arbitrary Unit");
  hPhiAngle->DrawNormalized();


  TCanvas *c3=new TCanvas("c3","c3",1600,600);
  c3->SetGrid(1);
  // c3->cd(1);
  // hslopeX->Draw();
  // c3->cd(2);
  // hslopeY->Draw();
  // c3->cd(3);
  hPos2D->Draw("colz");
  // c3->cd(4);
  // hCosmicPath->Draw();

  TCanvas *c4=new TCanvas("c4","c4",1200,600);
  c4->Divide(2,1);
  TLine *lscinti1=new TLine(SizeOfScinti[0],0,SizeOfScinti[0],SizeOfScinti[1]);
  TLine *lscinti2=new TLine(0,SizeOfScinti[1],SizeOfScinti[0],SizeOfScinti[1]);
  c4->cd(1);
  hScintiHitDistribution[0]->Draw("colz");
  lscinti1->Draw();
  lscinti2->Draw();
  c4->cd(2);
  hScintiHitDistribution[1]->Draw("colz");
  lscinti1->Draw();
  lscinti2->Draw();

  TCanvas *c5=new TCanvas("c5","c5",1200,600);
  c5->Divide(2,1);
  TArc *arc=new TArc(0.,0.,13.3,135.,405.);
  TLine *larc=new TLine(13.3/sqrt(2),13.3/sqrt(2),-13.3/sqrt(2),13.3/sqrt(2));
  c5->cd(1);
  hAnodeHitDistribution->Draw("colz");
  arc->SetFillStyle(0);
  arc->SetNoEdges(1);
  arc->Draw();
  larc->Draw();
  c5->cd(2);
  hCathodeHitDistribution->Draw("colz");
  arc->Draw();
  larc->Draw();

  fwrite->Write();

  cout<<"+++++++++++++++++++++++++"<<endl;
  cout<<" X-Range:("<<-1*nMulti*MidX<<","<<nMulti*MidX<<")"<<endl;
  cout<<" Y-Range:("<<-1*nMulti*MidY<<","<<nMulti*MidY<<")"<<endl;
  for(int ii=0;ii<nScinti;ii++){
    cout<<" Scinti Align:"<<ii+1<<endl;
    for(int jj=0;jj<4;jj++){
      cout<<jj+1<<"("<<Pos[ii][jj].x<<","<<Pos[ii][jj].y<<","<<Pos[ii][jj].z<<") ";
    }
    cout<<endl;
  }
  cout<<" "<<HitCosmicCounter<<" Event/"<<ntime<<" minites"<<endl;
  cout<<"+++++++++++++++++++++++++"<<endl;

}

/*----------------
  ベクトルの減法
 -----------------*/
void SubVector2D( Point2D vec1, Point2D vec2, Point2D *vec ){
  vec->x = vec1.x - vec2.x;
  vec->y = vec1.y - vec2.y;
}
void SubVector3D( Point3D vec1, Point3D vec2, Point3D *vec ){
  vec->x = vec1.x - vec2.x;
  vec->y = vec1.y - vec2.y;
  vec->z = vec1.z - vec2.z;
}
/*-------------
 Size of Vector
 --------------*/
double SizeOfVector(Point3D vec){
  return sqrt(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z);
}
/*----------------
 Scale Vector
 -----------------*/
void ScaleVector(Point3D *vec,double Scale){
  Point3D tmpvec;
  tmpvec.x = vec->x;
  tmpvec.y = vec->y;
  tmpvec.z = vec->z;
  double size = SizeOfVector(tmpvec);
  vec->x = vec->x/size*Scale;
  vec->y = vec->y/size*Scale;
  vec->z = vec->z/size*Scale;
}
/*------------------------
  （三次元）外積を求める
  ------------------------*/
void CrossProduct3D( Point3D vec1, Point3D vec2, Point3D *vec ){
  vec->x = vec1.y * vec2.z - vec1.z * vec2.y;
  vec->y = vec1.z * vec2.x - vec1.x * vec2.z;
  vec->z = vec1.x * vec2.y - vec1.y * vec2.x;
}
/*----------------------------
  法線ベクトルを求める
  pos1,pos2,pos3 : 平面の頂点
  ----------------------------*/
void NormalVector3D( Point3D pos1, Point3D pos2, Point3D pos3, Point3D *normal ){
  Point3D vec1, vec2;

  /* 法線ベクトルを求める */
  SubVector3D( pos2, pos1, &vec1 );
  SubVector3D( pos3, pos1, &vec2 );
  CrossProduct3D( vec1, vec2, normal );
}
/*----------------------------------------------------------
  （三次元）内積を求める
  ----------------------------------------------------------*/
double DotProduct3D( Point3D vec1, Point3D vec2 ){
  return ( vec1.x*vec2.x + vec1.y*vec2.y + vec1.z*vec2.z );
}
/*--------------------------
  直線と平面の交点を求める
  pos1 : 直線上のある点
  vec1 : 直線の方向ベクトル
  pos2 : 平面の頂点
  vec2 : 平面の法線ベクトル
  ---------------------------*/
int LinePolyCrossPoint( Point3D pos1, Point3D vec1, Point3D pos2, Point3D vec2, Point3D *pos ){
  double  a, b, t;
  Point3D vec;

  SubVector3D( pos2, pos1, &vec );
  a = DotProduct3D( vec, vec2 );
  b = DotProduct3D( vec1, vec2 );

  /* 平行なので交点なし */
  if( b == 0.0 )    return 0;
  /* 媒介変数ｔを求める */
  t = a/b;
  /* 直線の方程式に代入し、交点を求める */
  pos->x = pos1.x + vec1.x*t;
  pos->y = pos1.y + vec1.y*t;
  pos->z = pos1.z + vec1.z*t;
  /* 交点あり */
  return 1;
}

